
Solve = function(mat){
    var n = mat.length
    for (let i = 0; i < n; i++) {
        arr = mat.splice(i, n-i)
        arr.sort((a, b)=>{
            a = a[i]
            if (a<0){
                a = -a
            }
            b = b[i]
            if (b<0){
                b = -b
            }
            return b-a
        })
        mat = mat.concat(arr)
        Aii = mat[i][i]
        if (Aii == 0.0){
            return null
        }
        for (let j = i; j < n+1; j++) {
            mat[i][j] /= Aii
        }
        for (let j = i+1; j < n; j++) {
            Aji = mat[j][i]
            if (Aji==0.0){
                continue;
            }
            for (let k = i; k < n+1; k++) {
                mat[j][k] = -mat[i][k] + mat[j][k]/Aji
            }
        }
    }
    for (let i = 0; i < n; i++) {
        for(let j = 0; j < n-1-i; j++){
            Axx = mat[n-2-i-j][n-1-i]
            mat[n-2-i-j][n-1-i] = 0
            mat[n-2-i-j][n] -= Axx*mat[n-1-i][n]
        }
    }
    solve = new Float32Array(n)
    for (let i = 0; i < n; i++) {
        solve[i] = mat[i][n]
    }
    return {mat, solve}
}

function RandomMat(){
    n = Math.floor(Math.random()*5)+3
    // n = 3
    solve = new Float32Array(n)
    for (let i = 0; i < n; i++) {
        solve[i] = Math.floor((Math.random()*2-1)*100)
    }
    mat = new Array(n)
    for (let i = 0; i < n; i++) {
        mat[i] = new Float32Array(n+1)
    }
    for (let i = 0; i < n; i++) {
        mat[i][i] = 1
        mat[i][n] = solve[i]
    }
    for (let i = 0; i < 5; i++) {
        a = Math.floor(Math.random()*n)
        b = Math.floor(Math.random()*n)
        c = 1
        for (let j = 0; j < n+1; j++) {
            mat[a][j] += c*mat[b][j]
        }
    }
    return {mat, solve}
}

/**
 * (x1-x2)^2 + (y1-y2)^2 + (z1-z2)^2 = R^2
 * (x1-x2)(dx1-dx2)
 * (dx1-dx2)^2+(x1-x2)(d2x1-d2x2)
 * d2x/dt2 = sum((xi-xj)*fk)
 */

function Physics(){
    this.particle = new Array()
    this.constraint = new Array()
    this.dt = 0.01
    this.mat = null
}

function Particle(physics, x, y, z){
    this.physics = new WeakRef(physics)
    physics.particle.push(this)
    this.mem = null
    this.pos = new Float32Array([x, y, z])
    this.mass = 1
    this.F = new Float32Array(3)
    this.needUpdateV = true
    this.v = new Float32Array(3)
    this.onSimulate = null
}

Particle.prototype.Distance = function(particle){
    var sum = 0
    for(let i = 0; i < 3; i++){
        let sub = (this.pos[i] - particle.pos[i])
        sum += sub * sub
    }
    return Math.sqrt(sum)
}

function Constraint(physics, p1, p2){
    this.p = [p1, p2]
    this.len = p1.Distance(p2)
    this.physics = new WeakRef(physics)
    physics.constraint.push(this)
}

Physics.prototype.Simulate = function(){
    for(let p of this.particle){
        if (p.mem == null){
            p.mem = new Float32Array(3)
            for (let j = 0; j < 3; j++) {
                p.mem[j] = p.F[j]/p.mass
            }
        }
        if (p.onSimulate){
            p.onSimulate()
        }
    }
    const n = this.particle.length*3+this.constraint.length
    if(!this.mat || this.mat.length != n){
        this.mat = new Array(n)
        for(let i=0; i<n; i++){
            this.mat[i] = new Float32Array(n+1)
        }
    }

    const mat = this.mat
    for(let i=0; i<n; i++){
        mat[i][i] = 0
        mat[i][n] = 0
    }
    for(let i in this.particle){
        let p = this.particle[i]
        for(let j=0 ; j<3; j++){
            let k = 3*i+j
            mat[k][k] = -1
            mat[k][n] = -p.F[j]/p.mass
        }
    }
    const pn = this.particle.length
    for(let i in this.constraint){
        i = Number(i)
        let c = this.constraint[i]
        let p = [this.particle.indexOf(c.p[0]), this.particle.indexOf(c.p[1])]
        let bi = c.p[0].Distance(c.p[1])
        if (bi<=0){
            bi = 1
        }
        else{
            bi = c.len/bi * 1
        }
        // bi = (bi-1)/1024/16+1
        bi = 1
        for(let k=0; k<2; k++){
            if (c.p[k].needUpdateV){
                for(let j=0; j<3; j++){
                    let r = p[k]*3+j
                    mat[r][3*pn+i] = (c.p[k].pos[j]-c.p[(k+1)%2].pos[j])/c.p[k].mass
                }
            }
        }
        let r = 3*pn+i
        for(let j=0; j<3; j++){
            let sub = (c.p[0].v[j]-c.p[1].v[j])
            mat[r][n] -= sub*sub
        }
        for(let k=0; k<2; k++){
            for(let j=0; j<3; j++){
                mat[r][p[k]*3+j] = (c.p[k].pos[j]-c.p[(k+1)%2].pos[j]) * bi
            }
        }
    }
    var solve = Solve(mat)
    this.mat = solve.mat
    solve = solve.solve
    if(!solve){
        return
    }

    for(let p of this.particle){
        if (p.mem == null){
            p.mem = new Float32Array(3)
            for (let j = 0; j < 3; j++) {
                p.mem[j] = solve[3*i+j]
            }
        }
    }

    for(let i in this.particle){
        i = Number(i)
        let p = this.particle
        for(let j=0; j<3; j++){
            // p[i].pos[j] += p[i].v[j]*this.dt+solve[3*i+j]*this.dt*this.dt/2+(solve[3*i+j]-p[i].mem[j])*this.dt*this.dt/6
            p[i].pos[j] += p[i].v[j]*this.dt+solve[3*i+j]*this.dt*this.dt/2
            p[i].mem[j] = solve[3*i+j]
        }
    }

    for(let i in this.particle){
        i = Number(i)
        let p = this.particle
        for(let j=0; j<3; j++){
            if (p[i].needUpdateV){
                // p[i].v[j] += solve[3*i+j]*this.dt+(solve[3*i+j]-p[i].mem[j])*this.dt/2
                p[i].v[j] += solve[3*i+j]*this.dt
                p[i].v[j] *= (1-0.00005)
            }
        }
    }

    for(let i in this.constraint){
        i = Number(i)
        c = this.constraint[i]
        var vec = new Float32Array(3)
        var len = 0.0
        for(let j = 0; j < 3; ++j){
            vec[j] = c.p[0].pos[j] - c.p[1].pos[j]
            len += vec[j] * vec[j]
        }
        len = Math.sqrt(len)
        if(len > 0){
            len = 1.0 - c.len / len
        }
        var k0 = c.p[1].mass / (c.p[0].mass + c.p[1].mass)
        var k1 = c.p[0].mass / (c.p[0].mass + c.p[1].mass)
        if(!c.p[0].needUpdateV){
            k0 = 0.0
            k1 = 1.0
        }
        if(!c.p[1].needUpdateV){
            k0 = 1.0
            k1 = 0.0
        }
        if(!c.p[0].needUpdateV && !c.p[1].needUpdateV){
            k0 = 0.0
            k1 = 0.0
        }
        for(let j = 0; j < 3; ++j){
            c.p[0].pos[j] -= len * k0 * vec[j]
            c.p[1].pos[j] += len * k1 * vec[j]
        }
    }
}

function test(){
    var physics = new Physics()
    var p1 = new Particle(physics, 0, 0, 10)
    p1.needUpdateV = false
    var p2 = new Particle(physics, 1, 0, 0)
    p2.F = new Float32Array([0, 0, -10])
    p2.onSimulate = function(){
        console.log(this.pos[0], this.pos[2])
    }
    var c = new Constraint(physics, p1, p2)

    setInterval(()=>{
        physics.Simulate()
    }, 100)
}